#include <iostream>
#include "Eigen/Dense"
using namespace std;
using namespace Eigen;
#define Mb 2
#define Nb 3
void solverSMsimple(double AMatrix[Mb][Nb], double I[Mb]);
int main()
{
    double ZC = 1256, Geq1 = 4588.2, Geq2 = 78.54, Ieq1 = 13.62, vi = 4.2, Ieq2 = 12.63, Js = 9.123;

    double Gf[Mb][Nb] = {
        1 / ZC + Geq1, -Geq1, -Ieq1 + 2 * (vi) / ZC,
        -Geq1, Geq1 + Geq2, Ieq1 - Ieq2 + Js};
    double Unew[Mb] = {0};

    solverSMsimple(Gf, Unew);
    printf("[Unew]-1: ");
    for (int i = 0; i < Mb; i++)
    {
        printf("%.12lf ", Unew[i]);
    }
    printf("\n");

    //////////////////////////////////////
    Matrix<double, 2, 2> Gf2;
    Matrix<double, 2, 1> Ieq;
    Gf2 << 1 / ZC + Geq1, -Geq1,
        -Geq1, Geq1 + Geq2;
    Ieq << -Ieq1 + 2 * (vi) / ZC,
        Ieq1 - Ieq2 + Js;

    Matrix<double, 2, 1> Unew2;
    Unew2 = Gf2.inverse() * Ieq;
    printf("[Unew]-2: ");
    for (int i = 0; i < Mb; i++)
    {
        printf("%.12lf ", Unew2(i));
    }
    printf("\n");

    return 0;
}

void solverSMsimple(double AMatrix[Mb][Nb], double I[Mb])
{
    double temp[Mb] = {0};
    int k = 0;

    while (k < Mb - 1)
    {
        temp[k] = AMatrix[k][k];

        for (int j = k; j < Nb; j++)
        {
            AMatrix[k][j] = AMatrix[k][j] / temp[k];
        }
        for (int i = k + 1; i < Mb; i++)
        {
            // for (int j = k + 1; j < Na; j++)
            //{
            //	AMatrix[i][j] = AMatrix[i][j] - AMatrix[i][k] * AMatrix[k][j];
            // }
            // AMatrix[i][k] = 0;
            if (AMatrix[i][k] != 0)
            {
                for (int j = k + 1; j < Nb; j++)
                {
                    AMatrix[i][j] = AMatrix[i][j] - AMatrix[i][k] * AMatrix[k][j];
                }
            }
        }
        k = k + 1;
    }

    double Adiag = AMatrix[Mb - 1][Mb - 1];
    for (int j = Mb - 1; j <= Nb - 1; j++)
    {
        AMatrix[Mb - 1][j] = AMatrix[Mb - 1][j] / Adiag;
    }

    int kk = Mb - 1;
    while (kk > 0)
    {
        for (int i = 0; i <= kk - 1; i++) // or for (int i=0;i<kk-1;i++)?
        {
            temp[i] = AMatrix[i][kk];
            for (int j = kk; j <= Nb - 1; j++)
            {
                AMatrix[i][j] = AMatrix[i][j] - temp[i] * AMatrix[kk][j];
            }
        }
        kk = kk - 1;
    }

    for (int i = 0; i < Mb; i++)
    {
        I[i] = AMatrix[i][Nb - 1];
    }
}